package de.gratnik.sensors;

import java.util.List;

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.util.Log;
import android.widget.TextView;
import android.hardware.SensorManager;

public class SensorsActivity extends Activity implements SensorEventListener {

	public final String TAG = this.getClass().getSimpleName();
	SensorManager sm;
	TextView accelerometer = null;
	TextView linearAccelerometer = null;
	TextView gravity = null;
	TextView gyroscope = null;
	TextView rotation = null;
	TextView orientation = null;
	TextView position = null;
	TextView myaccel = null;
	float X = 0;
	float Y = 0;
	float Z = 0;
	float Vx = 0;
	float Vy = 0;
	float Vz = 0;
	float Ax = 0;
	float Ay = 0;
	float Az = 0;
	long Tprev = 0;
	long T0 = 0;

	float[] trueacceleration = new float[3];
	float[] accelerometervalues = new float[3];
	float[] orientationvalues = new float[3];
	
	float[] accelerometerSValues = new float[3];
	float[] gravitySValues = new float[3];
	float[] gravitySdifValues = new float[3];
	float[] giroscopeSValues = new float[3];
	
	
	Sensors sensors = new Sensors();

	/** Called when the activity is first created. */
	@Override
	public void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.main);

		accelerometer = (TextView) findViewById(R.id.accelerometer);
		linearAccelerometer = (TextView) findViewById(R.id.linear_accelerometer);
		gravity = (TextView) findViewById(R.id.gravity);
		gyroscope = (TextView) findViewById(R.id.gyroscope);
		rotation = (TextView) findViewById(R.id.rotation);
		orientation = (TextView) findViewById(R.id.orientation);
		position = (TextView) findViewById(R.id.position);
		myaccel = (TextView) findViewById(R.id.myaccel);

		sensors.Init(this, this);
		

	}

	

	protected void onResume() {
		super.onResume();
		
		X = 0;
		Y = 0;
		Z = 0;
		Vx = 0;
		Vy = 0;
		Vz = 0;
		Tprev = 0;
		T0 = 0;
	}

	protected void onPause() {
		super.onPause();
		
	}

	


	public void onAccuracyChanged(Sensor arg0, int arg1) {

	}


	public void onSensorChanged(SensorEvent e) {
		long time = System.currentTimeMillis();

		if (e.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
			Log.i(TAG, String.format("ACCELEROMETER time=%s, sensortime=%s",
					String.valueOf(time), String.valueOf(e.timestamp)));
			StringBuilder sb = new StringBuilder();
			for (float value : e.values) {
				sb.append(value);
				sb.append(" ");
			}
			Log.i(TAG, sb.toString());
			accelerometer.setText(String.format("X=%.1f Y=%.1f Z=%.1f",
					e.values[0], e.values[1], e.values[2]));
			
			accelerometerSValues[0] = e.values[0];
			accelerometerSValues[1] = e.values[1];
			accelerometerSValues[2] = e.values[2];
		}

		if (e.sensor.getType() == Sensor.TYPE_GRAVITY) {
			Log.i(TAG,
					String.format("GRAVITY time=%s, sensortime=%s",
							String.valueOf(time), String.valueOf(e.timestamp)));
			StringBuilder sb = new StringBuilder();
			for (float value : e.values) {
				sb.append(value);
				sb.append(" ");
			}
			Log.i(TAG, sb.toString());
			gravity.setText(String.format("X=%.1f Y=%.1f Z=%.1f", e.values[0],
					e.values[1], e.values[2]));
			
			gravitySdifValues[0] = e.values[0] - gravitySValues[0];
			gravitySdifValues[1] = e.values[1] - gravitySValues[1];
			gravitySdifValues[2] = e.values[2] - gravitySValues[2];
			
			gravitySValues[0] = e.values[0];
			gravitySValues[1] = e.values[1];
			gravitySValues[2] = e.values[2];
			
			
		}
		if (e.sensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION) {
			Log.i(TAG, String.format(
					"LINEAR_ACCELEROMETER time=%s, sensortime=%s",
					String.valueOf(time), String.valueOf(e.timestamp)));
			StringBuilder sb = new StringBuilder();
			for (float value : e.values) {
				sb.append(value);
				sb.append(" ");
			}
			Log.i(TAG, sb.toString());
			linearAccelerometer.setText(String.format("X=%.1f Y=%.1f Z=%.1f",
					e.values[0], e.values[1], e.values[2]));

			if (T0 == 0) {
				T0 = e.timestamp;
			}

			if (Tprev == 0) {
				Tprev = e.timestamp;
			} else {

				float deltaT = (float) (e.timestamp - Tprev) / 1000000000;
				if (deltaT > 0.01 || true) {

					accelerometervalues[0] = e.values[0];
					accelerometervalues[1] = e.values[1];
					accelerometervalues[2] = e.values[2];

					trueacceleration[0] = (float) (accelerometervalues[0]
							* (Math.cos(orientationvalues[2])
									* Math.cos(orientationvalues[0]) + Math
									.sin(orientationvalues[2])
									* Math.sin(orientationvalues[1])
									* Math.sin(orientationvalues[0]))
							+ accelerometervalues[1]
							* (Math.cos(orientationvalues[1]) * Math
									.sin(orientationvalues[0])) + accelerometervalues[2]
							* (-Math.sin(orientationvalues[2])
									* Math.cos(orientationvalues[0]) + Math
									.cos(orientationvalues[2])
									* Math.sin(orientationvalues[1])
									* Math.sin(orientationvalues[0])));
					trueacceleration[1] = (float) (accelerometervalues[0]
							* (-Math.cos(orientationvalues[2])
									* Math.sin(orientationvalues[0]) + Math
									.sin(orientationvalues[2])
									* Math.sin(orientationvalues[1])
									* Math.cos(orientationvalues[0]))
							+ accelerometervalues[1]
							* (Math.cos(orientationvalues[1]) * Math
									.cos(orientationvalues[0])) + accelerometervalues[2]
							* (Math.sin(orientationvalues[2])
									* Math.sin(orientationvalues[0]) + Math
									.cos(orientationvalues[2])
									* Math.sin(orientationvalues[1])
									* Math.cos(orientationvalues[0])));
					trueacceleration[2] = (float) (accelerometervalues[0]
							* (Math.sin(orientationvalues[2]) * Math
									.cos(orientationvalues[1]))
							+ accelerometervalues[1]
							* (-Math.sin(orientationvalues[1])) + accelerometervalues[2]
							* (Math.cos(orientationvalues[2]) * Math
									.cos(orientationvalues[1])));

					// float Ax = trueacceleration[0];
					// float Ay = trueacceleration[1];
					// float Az = trueacceleration[2];
					
					
					if (Ax*e.values[0] < 0 ) {
						Vx = 0;
					}

					Ax = e.values[0];
					Ay = e.values[1];
					Az = e.values[2];
					
					

					float th = (float) 0.3;
					if (Math.abs(Ax) < th) {
						Ax = 0;
					}
					if (Math.abs(Ay) < th) {
						Ay = 0;
					}
					if (Math.abs(Az) < th) {
						Az = 0;
					}

					

					Vx = Ax * deltaT + Vx;

					Vy = Ay * deltaT + Vy;

					Vz = Az * deltaT + Vz;

					X = (float) (X + Vx * deltaT + 0.5 * Ax * deltaT * deltaT);
					
					if (X > 0.5 || X < -0.5) {
						Vx =0;
					}
					
					Y = (float) (Y + Vy * deltaT + 0.5 * Ay * deltaT * deltaT);
					Z = (float) (Z + Vz * deltaT + 0.5 * Az * deltaT * deltaT);
					Tprev = e.timestamp;
					position.setText(String
							.format("X=%.2f Y=%.2f Z=%.2f T=%.2f F=%.2f 		Vx=%.2f Vy=%.2f Vz=%.2f",
									X, Y, Z,
									(float) (e.timestamp - T0) / 1000000000,
									1 / deltaT, Vx, Vy, Vz));

					Log.w(TAG, sb.toString());
					
					float myX = e.values[0];
					float myY = e.values[1];
					float myZ = e.values[2];
					
					if (Math.abs(giroscopeSValues[0]) > 0.07) {
						myX = 0;
					}
					if (Math.abs(giroscopeSValues[1]) > 0.07) {
						myY = 0;
					}
					if (Math.abs(giroscopeSValues[2]) > 0.07) {
						myZ = 0;
					}
					
					if (Math.abs(gravitySdifValues[0]) > 0.07) {
						myX = 0;
					}
					if (Math.abs(gravitySdifValues[1]) > 0.07) {
						myY = 0;
					}
					if (Math.abs(gravitySdifValues[2]) > 0.07) {
						myZ = 0;
					}
					
					
					
					
					myaccel.setText(String
							.format("X=%.2f Y=%.2f Z=%.2f", myX, myY, myZ));
				}
			}

		}

		if (e.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
			Log.i(TAG,
					String.format("GYROSCOPE time=%s, sensortime=%s",
							String.valueOf(time), String.valueOf(e.timestamp)));
			StringBuilder sb = new StringBuilder();
			for (float value : e.values) {
				sb.append(value);
				sb.append(" ");
			}
			Log.i(TAG, sb.toString());
			gyroscope.setText(String.format("X=%.3f Y=%.3f Z=%.3f",
					e.values[0], e.values[1], e.values[2]));
			
			giroscopeSValues[0] = e.values[0];
			giroscopeSValues[1] = e.values[1];
			giroscopeSValues[2] = e.values[2];
		}

		if (e.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
			Log.i(TAG,
					String.format("ROTATION time=%s, sensortime=%s",
							String.valueOf(time), String.valueOf(e.timestamp)));
			StringBuilder sb = new StringBuilder();
			for (float value : e.values) {
				sb.append(value);
				sb.append(" ");
			}
			Log.i(TAG, sb.toString());
			rotation.setText(String.format("X=%.1f Y=%.1f Z=%.1f", e.values[0],
					e.values[1], e.values[2]));
		}

		if (e.sensor.getType() == Sensor.TYPE_ORIENTATION) {
			Log.i(TAG,
					String.format("ORIENTATION time=%s, sensortime=%s",
							String.valueOf(time), String.valueOf(e.timestamp)));
			StringBuilder sb = new StringBuilder();
			for (float value : e.values) {
				sb.append(value);
				sb.append(" ");
			}
			Log.i(TAG, sb.toString());
			orientation.setText(String.format("X=%.1f Y=%.1f Z=%.1f",
					e.values[0], e.values[1], e.values[2]));

			orientationvalues[0] = e.values[0];
			orientationvalues[1] = e.values[1];
			orientationvalues[2] = e.values[2];
		}

	}
}